/*
 * @Description: 订阅imu数据
 * @Author: Zhijian Qiao
 * @Date: 2019-06-14 16:44:18
 */
#include "subscriber/velocity_subscriber.hpp"
#include "glog/logging.h"

namespace avp_slam {
    VelocitySubscriber::VelocitySubscriber(ros::NodeHandle &nh, std::string topic_name, size_t buff_size,
                                           const Eigen::Matrix4f &car_to_imu)
            : nh_(nh) {
        subscriber_ = nh_.subscribe(topic_name, buff_size, &VelocitySubscriber::msg_callback, this);
        LOG(INFO) << "VelocitySubscriber topic_name: " << topic_name << std::endl;
        car_to_imu_ = car_to_imu;
    }

    void VelocitySubscriber::msg_callback(const gazebo_msgs::LinkStatesConstPtr &links_ptr) {
        VelocityData velocity_data;
//        velocity_data.time = twist_msg_ptr->header.stamp.toSec();
//fixme 速度时间戳
        velocity_data.time = ros::Time::now().toSec();
        LOG(INFO) << std::fixed << "velocity time: " << velocity_data.time << ", linear_velocity.x "
                  << velocity_data.linear_velocity.x << ", last_time: " << last_time_;
        if (velocity_data.time == last_time_)
            return;
        last_time_ = velocity_data.time;

        int indexLeft = -1;
        std::vector<std::string> names = links_ptr->name;
        for (size_t i = 0; i < names.size(); i++)
            if (names[i] == "robot_1::wheel_left_link")
                indexLeft = i;
        geometry_msgs::Twist left_wheel_twist = links_ptr->twist[indexLeft];
        geometry_msgs::Twist right_wheel_twist = links_ptr->twist[indexLeft + 1];

        velocity_data.linear_velocity.x = (left_wheel_twist.linear.x + right_wheel_twist.linear.x) / 2.0;
        velocity_data.linear_velocity.y = (left_wheel_twist.linear.y + right_wheel_twist.linear.y) / 2.0;
        velocity_data.linear_velocity.z = (left_wheel_twist.linear.z + right_wheel_twist.linear.z) / 2.0;

        velocity_data.angular_velocity.x = (left_wheel_twist.angular.x + right_wheel_twist.angular.x) / 2.0;
        velocity_data.angular_velocity.y = (left_wheel_twist.angular.y + right_wheel_twist.angular.y) / 2.0;
        velocity_data.angular_velocity.z = (left_wheel_twist.angular.z + right_wheel_twist.angular.z) / 2.0;

        if (abs(velocity_data.linear_velocity.x) > 0.0 || abs(velocity_data.linear_velocity.y) > 0.0 ||
            abs(velocity_data.linear_velocity.z) > 0.0)
            nonZeroLast = velocity_data;

        buff_mutex_.lock();
        new_velocity_data_.push_back(velocity_data);
        buff_mutex_.unlock();
    }

    void VelocitySubscriber::ParseData(std::deque<VelocityData> &velocity_data_buff) {
        buff_mutex_.lock();
        if (new_velocity_data_.size() > 0) {
            velocity_data_buff.insert(velocity_data_buff.end(), new_velocity_data_.begin(), new_velocity_data_.end());
            new_velocity_data_.clear();
        }
        buff_mutex_.unlock();
    }
}